% there are many ways to solve the problem:
% 1: powered swing up, so that pendulum reaches 0 angle and 0 velocity at the end of
% powered cart movement
% 
% 2: swing up to some small angle, at which point the balancing mode switches on

%3: swing up with unpowered glide to 0/0 angle/velocity

% 4; multiple node, overlapping to form comples force profile
% 
% 5: multiple nodes, acted sequenctially

% hypothosis: starting with strictly sequenyial nodes, then moving closer
% in time to refine performance

%Experiment 1:  Learn an on/off force pulse that puts the pendulum at pi
%with a managable velocity. velocity to be determined by experimentation.
%For the first run choose an arbitrary velocity
%-One way to check for "managability" is to have the pendulum arrive at 0
%radians with a velocity "close to" 0

% Feedback sensor data: 3 dimensions: Angular position, aNGULAR VELOCITY,
% angular acceleration.
% each parameter varies from -min to +max

%Artuculator input: Specified as a pair of start/stop accelleration pulses.
%Time is measured from T0, which is the time of occurance of sensor data
%landmarks
%4 dimensions: AF (Amplitude of applied force( same for each pulse)), T
%(Pulse periods), ST1 (start time delay from T0 of pulse 1),ST2 (STart time
%delay of pulse 2) Start time of pulse 2 = T0+ST1+T+ST2)

% Use 3D interpolating lookup tables to record learned mapping between
% sensor data and articulators

%First step in defining lookup tables: specify the coordinates of data
%points.  ( called break points).  Values between known brak points will be
%interpolated. Break points correspond to the trigger values of sensor
%data, like all values=0, AP crossing pi, AV=0.  The data in the lookup
%tables corespond to the learned articulator parameters
clear
TD=.05;%time increment of simulation
TARGET=pi;% goal: have the pendulum reach pi radians with specified velocity
AV_THRESH=.02;% target angular velocity when crossing target
MAX_A=300;% max acceleration
MAX_V=100;% max velocity
MAX_P=10;% max position,
MASS=5+1;% 5 kg cart mass, 1 kg pendulum mass
SYS=0.18;% factor used to calculate the end velocity due to applied acceleration
CP_D=(.975*MAX_P);% desired max cart position when adjusting parameters
%Task: Apply pulse to cart, measure Ap when AV=0.  Maximize AP until AP==pi
%For now, only vary ST2. A quescient point is defined, AF=200, T= .2728,
%ST1=0 when AP=AV=AA=0 ( the initial state with cart at rest and pendulum
%at 0 radians with no angular velocity or acceleration) 


%initial angular position and velocity
API=-.99*pi;
AVI=0;
TI=.001;% initial time.  this is the minimum start time for input.  Needed to allow initial conditions to take hold.


%Breakpoint coordinates correspond to sets of sensor data that trigger
%actuator response
%APBP=[API 1]; % angular position break points-# of elements= 3 of rows of slizes
[APBP APorder]=sort([API 1]);
AVBP=[AVI 1]; % Angular velocity break points-# of elements=# of columns of slilces
AABP=[0 1];
TLDAF=zeros(length(APBP),length(AVBP),length(AABP));% 3D array
TLDT=TLDAF;
TLDST2=TLDT;
%Get initial acceleration of system, given initial angular velocity and
%angular position
[t,x,ap,av,ac,cp,d2,d3,d4,d5]=sim('ipend2',1);
[AABP AAorder]=sort([ac(1) 1]); % Angular acceleration break points--# of elemetns= # horizontal slices
%Set initial guesses of cart motion that will amke pendulum reach target
%Want total cart distance=5 ( for example)
%Ve=t_0*(.18*af_0);% velocity t end of first acceleration pulse
%CP=Ve*(t_0+st2_0);% cart position due to acceleration pulses + cruise stage
%--------------------------------------------------------------------------
%CP=0.18*t_0^2*af_0+0.18*t_0*af_0*st2_0
%dCP/dt_0=.36*t_0*af_0+0.18*af_0*st2_0
%dCP/af_0=0.18*t_0^2+0.18*st2_0
%dCP/st2_0=0.18*t_0*af_0
%dCP/dt_0 has the highest change, when t_0=1, st2_0=.1, af_0=100, use as
%rule of thumb
%--------------------------------------------------------------------------
% the .18 is determined experimentally, by applying a AF of 100 and
% measuring cart acceleration.  Thi is the "hefting " stage, getting the
% feel for the weight and inertia of the system.
st_0=0;
st2_0=0;% P0 dimension 1
t_0=.51;% P0 dimension 2
af_0=-360;
TLDAF(APorder(1),1,AAorder(1))=af_0;
TLDT(APorder(1),1,AAorder(1))=t_0;
TLDST2(APorder(1),1,AAorder(1))=st2_0;

%init_slice=zeros(length(APBP));
% TLDAF(2,2,2)=0;%200;%table lookup for applied foirce magnitude
% TLDAF(1,2,:)=300;% for unknown or dont-care values, use : operator
% TLDST2(2,2,2)=0;%st2_0;% table lookup data for AF ST2 initialized to 0 at quescient point, arbitrarily set to 1  at sensor space extremes
% TLDT(2,2,2)=0;%t_0;% table lookup data for T (.2728)
% TLDT(1,2,:)=1;
%%
%NON ZERO STARTING ANGLE, ZERO VELOCITY
RESULT=0;
PARAM_INC=1;% start with large increment, reduce until appropriate value is found for all parameters
PARAM_IDX=0;% [af_0,t_0,st2_0] is the cart-movement-space vector.
AV=[];% record the angular velocity values at P_0 and for partials
AP=[];
% Initialize the simulation parameters to equal point P_0
ST2=st2_0;
T=t_0;
AF=af_0;
while 1
    RESULT=0;
    [t,x,ap,av,ac,cp,d2,d3,d4,d5]=sim('ipend2',5);
    ind=find(round(t*1000)>(TI*1000));% Ignore a brief interval of initial time.
    t=t(ind);
    ap=ap(ind);
    av=av(ind);
    ac=ac(ind);
    ap=unwrap(ap);
    if ap(1)<-pi/2
        ap=ap+2*pi;
    end
    
    clf
    subplot(3,1,1)
    plot(t,av)
    hold
    plot([t(1) t(end)],[0 0],'r')
    axis([1 2.5 0 .3])
    subplot(3,1,2)
    plot(t,av)
    hold
    plot([t(1) t(end)],[0 0],'r')  
   % axis([1 2.5 3 pi])
    subplot(3,1,3)
    plot(t,ac)
    hold
    plot([t(1) t(end)],[0 0],'r') 
    axis([1 2.5 -.5 0])
    
%     DIV=3.839*exp(-1.125*ST2)-0.5871*exp(-7.071*ST2);% Function found by curv fitting data set: ST2(x)=[0 .1 .2 .25]; DIV(empirically derived)=[3.252 pi 2.922 2.798]
%     DIV=2.7;
    %if start angle >=.65pi use:[ap_ind,ap_t0]=crossing(ap-pi,t);
    %if angle == 0 use [ap_ind,ap_t0]=crossing(ap-pi/4,t);
    [ap_ind,ap_t0]=crossing(ap-pi,t); % start with parameters tah cause angle to cross pi, then find when angle ofpendulum crosses pi/4 to determine pulse midpoint
    [aa_ind,aa_t0]=crossing(ac,t);% find times of angular acceleration 0 crossing  
    [av_ind,av_t0]=crossing(av,t);% find times of angular velocity 0 crossing      
    if(~isempty(ap_t0))% if pendulum *does* cros pi
        ind=ind(1);% only concerned abbout first  incident
        dt=(ap_t0(1)-t(ap_ind(1)))./(t(ap_ind(1)+1)-t(ap_ind(1)));% time increments normalized to 1, at what fraction of time step does zero crossing occur?The denominator is the simulation time step, which varies from sample to sample
        av0= av(ap_ind(1))+dt.*(av(ap_ind(1)+1)-av(ap_ind(1))); 
    end
    maneuver_time=st_0+TLDST2(APorder(1),1,AAorder(1))+2*TLDT(APorder(1),1,AAorder(1))+TI; % end time of cart position maneuver
    
    TLDST2(APorder(1),1,AAorder(1))=ST2;
%api=-.99*pi;ST2=0;AF=[-360 -340 -300 -200 -100 -50];TM=[.84 .84 .84 .84 .84];
%api=-.97*pi;ST2=0;AF=[-360 -340 -300 -200 -100 -50];TM=[.84 .84 .84 .84 .84 .88];
%api=-.95*pi;ST2=0;AF=[-360 -340 -300 -200 -100 -50];TM=[.86 .86 .86 .86 .889 .92];
%api=-.90*pi;ST2=0;AF=[-360 -340 -300 -200 -100 -50];TM=[.9210 .9210 .9210 .9230 .947 .987];
%api=-.85*pi;ST2=0;AF=[-360 -340 -300 -200 -100 -50];TM=[.9531 .9532 .9533 .9585 .9810 1.0150];
%api=-.80*pi;ST2=0;AF=[-360 -340 -320 -300 -280 -260 -240 -220 -200 -180 -160 -140 -120 -100 -80 -60];TM=[.9788 .9788 .9789 .9792 .9797 .9805 .9814 .9825 .9840 .9865 .989 .9933 .998 1.0043 1.0131 1.0237];
%api=-.75*pi;ST2=0;AF=[-360 -350 -340 -320 -300 -280 -260 -240 -220 -200 -180 -160];TM=[.9965 .9966 .9966 .9969 .9975 .9979 .9986 .9995 1.001 1.0022 1.0045 1.0069]
%api=-.70*pi;ST2=0;AF=[-360 -340 -300 -260 -200 -150];TM=[1.0087 1.0088 1.0093 1.0109 1.0142 1.0211];
%api=-.65*pi;ST2=0;AF=[-360 -340 -300 -260 -250];TM=[1.019 1.022 1.035 1.0696 1.0895];
%%%api=-2*pi/3;ST2=0;AF=[-360 -340 -320 -300-280 -260 -240 -220 -200 -180 -160];AF=[.9055 .9055 .9072 .91 .9114 .9165 .9229 .9319 .9356 .9515 1.0129];
%api=0;ST2=0;AF=[-360 -355 -350 -345 -340 -335 -300 -280 -260 -240 -220 -200 -180 -160 -140 -120];TM=[1.0359 1.0379 1.0406 1.0425 1.0450 1.0476 1.0672 1.0809 1.0957 1.1136 1.1348 1.1612 1.1941 1.2379 1.3018 1.4120]
     %when angle >=.65*pi use:TLDT(1,1,AAorder(1))=.9788*ap_t0(1)-.005-ST2/2;%T; 
     TLDT(APorder(1),1,AAorder(1))=.84*ap_t0(1)-.005-ST2/2;%T; 
    
    % angle=0
    %TLDT(1,1,AAorder(1))=TLDT(1,1,AAorder(1))*.9;%(1figure.25*ap_t0(1)-.005-ST2)/1.5;%T; AF=[-300 -260 -200 -160];TM=[.94 .969 1.028 1.095]   
    %tt=[tt TLDT(1,1,AAorder(1))];
    TLDAF(APorder(1),1,AAorder(1))=AF;   
    
    %Switch applied force when angularclf position crosses pi
    %If applied force 
    
    
    
    
    
    
%     %NEW ANALYSIS PROCEDURE
%     %1.pendulum does not cross pi, velocity =0 after manauver
%     %--Undershoot: use gradient ascent to approach goal
%     %2.pendulum does not cross pi, velocity =0 before manauver end
%     %--Undershoot caused by slow, long cart movement.  Adjust integral of
%     %cart movement to increase magnitude and reduce time
%     %3.pendulum crosses pi before manauver is done
%     %--Overpowered: reduce times proportionately to end before time of
%     %crossing
%     %4. pendulum crosses pi after manauver at some high velocity
%     %--Overshoot: use gradient descent to approach goal
%     %5: goal is reached: pendulum crosses pi after manauver at a low velocity
%     %--Target acquired: store manauver in memory    
%     if isempty(t0)% pendulum doesn't cross pi.
%         [ind,t0]=crossing(av,t);% find the zero velocity time.   
%         if t0(1)>maneuver_time% if velocity = 0 after manauver
%             RESULT=1;
%         elseif t0(1)<=maneuver_time% if velocity ~=0 before manauver end, 
%             RESULT=2; 
%         end
%     elseif ~isempty(t0) % then pendulum crosses pi 
%         if t0(1)<=maneuver_time% pendulum crosses pi before manauver is done
%             RESULT=3;
%         end
%         if t0(1)>maneuver_time
%             RESULT=4;
%             if av(ind)<AV_THRESH
%                 RESULT=5;
%                 disp('..............TARGET REACHED......................');
%                  pause
%             end
%         end
%     end% if ~isempty(t0)
%     %Record the components of P_0      
%     switch RESULT
%         case 1% Undershoot: use gradient ascent to approach goal
%             %we want to adjust cart movement to minimize av0
%             %points in cart-movement-space are triplets of [af_0 t_0 st2_0]
%             %find an approximation of the gradient:increment each dimension by some
%             %small amount, to get the partials.  
% 
%             %[Desired change]/GRAD_MAGNITUDE=multiplicative factor F.  Multiply
%             %partials by F to get delta of each parameter.  Desired change is some
%             %small fixed step
% 
%             %First: increment af_0. increment is some fixed, small step, use the
%             %same step for each parameter.  We always want to be crossing pi, it's
%             %just a question of how fast. Want to minimize velociy at crossing
%             %time.
% 
%             %FIRST: find a parameter increment that, when used for each
%             %partial derivative, doeas not cause overshoot.
%             
%             % if we are here, and PARAM_IDX==0, then we have just
%             % gotten results from sim at P_0
%             if PARAM_IDX==0% then we just found response at initial point
%                 AP=[];% record the angular position across trials ( should approach pi)
%                 PARAM_IDX=1;% start adjusting parameter 1
%             end% if PARAM_IDX==0            
% % %             dt=(t0(1)-t(ind))./(t(ind+1)-t(ind));% time increments normalized to 1, at what fraction of time step does zero crossing occur?The denominator is the simulation time step, which varies from sample to sample
% % %             av0= av(ind)+dt.*(av(ind+1)-av(ind));%find velocity at pi crossing time              
% % %             AP=[AP ap(1)];% record the change in angular position measured at time 0    
%             switch PARAM_IDX
%                 case 1
%                     AP=[AP ap(ind(1))];% record the angular position resulting from P_0 parameter values
%                     AF=af_0+PARAM_INC*sign(af_0);
%                     T=t_0;
%                     ST2=st2_0;
%                     PARAM_IDX=2; 
%                 case 2
%                     AP=[AP ap(ind(1))];% record the angular position as result of adjusting parameter 1
%                     T=t_0+PARAM_INC;
%                     PARAM_IDX=3;                    
%                     CP=0.18*T^2*AF+0.18*T*AF*ST2;% cart position caused by parameters settings
%                     if abs(CP)>(.5*MAX_P)% then incrementing T by PARAM_INC will cause the cart to move past max cart position change
%                         %see if reducing the magnitude of AF is possible.
%                         %Leave the time increment if possible to allow for
%                         %exploraqtion of temporal parameters
%                         af_0=sign(af_0)*(50*(.5*MAX_P))/(9*T*(T+ST2));% change P_0 location to accomodate time increment
%                         PARAM_IDX=0;% restart gradient procedure with new af_0
%                     end% if CP>(.99*MAX_P)
%                     AF=af_0;
%                     ST2=st2_0;                    
%                 case 3
%                     AP=[AP ap(ind(1))];% record the angular position as result of adjusting parameter 2
%                     ST2=st2_0+PARAM_INC;
%                     AF=af_0;
%                     T=t_0;
%                     PARAM_IDX=4;            
%                 otherwise
%                     AP=[AP ap(ind(1))];% record the angular position as result of adjusting parameter 3
%                     PARAM_IDX=0;
%             end% switch PARAM_IDX   
%         case 2% Undershoot caused by slow cart movement.  Maintain cart position change, but increase AF
%             CP=0.18*T^2*AF+0.18*T*AF*ST2;% cart position change caused by current parameters.
%             % Incrementall increase AF & reduce T, maintain CP until
%             % desired response is achieved
%             dAF=1.5;
%             AF=AF*dAF;
%             C=CP/(.18*AF);
%             %ST2=ST2*dAF;% reduce ST2 arbitrarily by dAF, could use any fraction, even increase it if desired
%             %T=.5*(sqrt(ST2^2+4*C)-ST2);
%             %T=T*1.5;
%             t_0=T;% change P_0 to new initial value
%             af_0=AF;
%             st2_0=ST2;
%         case 3% Overpowered: reduce AF incrementally until pendulum no longer crosses target
%             %af_0=af_0*1.1; % change P_0 to new initial value
%             AF=af_0;
%         case 4% Overshoot: use gradient descent to approach goal
%         case 5% Target acquired: store manauver in memory 
%         otherwise
%             disp('..................UNHANDLED RESULT....................')
%             pause
%     end% switch
%     TLDST2(1,1,AAorder(1))=ST2;
%     %-3*pi/4
%     %TLDT(1,1,AAorder(1))=.9975*ap_t0(1)-.005-ST2/2;%T; AF=[-300 -260 -200 -160];TM=[.995 .9975 1.001 1.006]
%     
%     % angle=0
%     TLDT(1,1,AAorder(1))=1.095*ap_t0(1)-.005-ST2/2;%T; AF=[-300 -260 -200 -160];TM=[.94 .969 1.028 1.095]   
%     
%     %tt=[tt TLDT(1,1,AAorder(1))];
%     TLDAF(1,1,AAorder(1))=AF;    
end% while 1
%%
STOP=0;
AP0=[];
delta_ST2=0;
delta_T=0;
delta_AF=0;
T_INC=.1;% amount to increment parameters when calculating gradient
ST2_INC=.1;
AF_INC=.1;
idx=1;
STO=[];
prev_st2=[];% record the last good value tyhat does not cause overshoot
prev_t=[];
prev_af=[];
prev_AP0=[];
T_EPSILON=.1;
ST2_EPSILON=.1;
AF_EPSILON=.1;
inc_inc=0;% adjust increment of gradient paramters
grad_inc=0;% adjust increment of gradient traverse
ADJ_FACTOR=1;% how to increase/ecrese inc_inc as we approach desired target
ADJUSTING_INC=0;% are we currently adjusting increment?
prev_T_INC=0;
prev_ST2_EPSILON=0;
prev_T_EPSILON=0;
prev_AF_EPSILON=0;
while ~STOP
    %calculate the initial response, the anchor point
    t0=[];    
    [t,x,ap,av,ac,cp,d2,d3,d4,d5]=sim('ipend2',3);
    ind=find(round(t*1000)>(TI*1000));% remove the initial data
    t=t(ind);
    ap=ap(ind);
    av=av(ind);
    ac=ac(ind);
    [ind,t0]=crossing(abs(unwrap(ap))-pi,t);% interpolated zero crossing of angular position
    % stop if we cross pi at a low velocity.  What is low?  determine
    % experimentally. 0.3 radians/s seems to be the lowest we can get 
    if(~isempty(t0))% if pendulum *does* cros pi
        ind=ind(1);% only concerned abbout first  incident
        dt=(t0(1)-t(ind))./(t(ind+1)-t(ind));% time increments normalized to 1, at what fraction of time step does zero crossing occur?The denominator is the simulation time step, which varies from sample to sample
        av0= av(ind)+dt.*(av(ind+1)-av(ind));        
        if abs(av0)<.2% then pendulum crosses pi with a very low velocity. .1743 seems to be the achievable limit
            STOP=1;
            break;
        end
    end
    [ind,t0]=crossing(av,t);% t0=interpolated zero crossing time of angular velocity
    %desired behavior: 
    %--velocity very close to 0, unwrapped angle of occurance <=pi
    %detect velocity at pi crossing, minimize velocity
    if (isempty(t0))||any(abs(unwrap(ap))>TARGET)% then the pendulum never comes to zero angular velocity, reduce the current parameter under test until it does, then set as parameter max
        if idx>1% then ST2, T or AF inc caused overshoot
            idx=idx-1;
            prev_T_INC=T_INC;
            T_INC=(1-exp(-(.4*inc_inc+1.5)))*T_INC;
            ST2_INC=T_INC;
            AF_INC=T_INC;
            inc_inc=inc_inc+ADJ_FACTOR;
            ADJUSTING_INC=1;
            ADJ_FACTOR=ADJ_FACTOR/2;
        elseif idx==1 % then gradient inc caused overshoot
            idx=4;
            t_0=prev_t;
            st2_0=prev_st2;
            AP0=prev_AP0;
            prev_ST2_EPSILON=ST2_EPSILON;
            prev_T_EPSILON=T_EPSILON;
            ST2_EPSILON=(1-exp(-(.4*grad_inc+1.5)))*ST2_EPSILON;
            T_EPSILON=(1-exp(-(.4*grad_inc+1.5)))*T_EPSILON;
            grad_inc=grad_inc+ADJ_FACTOR;
            ADJ_FACTOR=1.1*ADJ_FACTOR;
            STO=STO(1:end-1,:);
            ADJUSTING_INC=1;
        end% if idx>1
    else
        if ADJUSTING_INC && (idx>1)% in the prvious trial, we were undershootinging target, and adjusting gradient parameter increments
            % now we are overshooting, 
            %ST2 inc caused overshoot
            idx=idx-1;
            T_INC=prev_T_INC;
            inc_inc=inc_inc+ADJ_FACTOR;
            T_INC=(1-exp(-(.4*inc_inc+1.5)))*T_INC;
            ST2_INC=T_INC;
            ADJUSTING_INC=1;
        end% if ADJUSTING_INC && (idx>1)  
        if ~ADJUSTING_INC
            prev_st2=TLDST2(1,1,AAorder(1));
            prev_t=TLDT(1,1,AAorder(1));
        end
        ind=ind(1);% only interested in first incident
        dt=(t0(1)-t(ind))./(t(ind+1)-t(ind));% time increments normalized to 1, at what fraction of time step does zero crossing occur?
        % ap0 is the parameter we want to maximize , but stay less than
        % than TARGET
        ap0= ap(ind)+dt.*(ap(ind+1)-ap(ind));% angular positions at times when angular velocities =0
        %stop if we cross zero velocity at an ngle clsoe to pi.  What is
        %close? determine experimentally.  0.98*pi seems to be the closest
        %we can get
%         if abs(ap0) >=(.9798*TARGET)% .979 seems to be the achievable limit
%             STOP=1;
%             break;
%         end  
        if ADJUSTING_INC && (idx==1) % then gradient inc caused undershoot
            idx=4;
            t_0=prev_t;
            st2_0=prev_st2;
            ST2_EPSILON=prev_ST2_EPSILON;
            T_EPSILON=prev_T_EPSILON;
            grad_inc=grad_inc+1;
            ST2_EPSILON=(1-exp(-(.4*grad_inc+1.5)))*ST2_EPSILON;
            T_EPSILON=(1-exp(-(.4*grad_inc+1.5)))*T_EPSILON;
            AP0=prev_AP0;            
            STO=STO(1:end-1,:);
        else
            AP0=[AP0 ap0]; % take the first extrema, ignore any others 
        end% if ADJUSTING_INC && (idx==1            
        
    end% if (isempty(t0))||any(abs(unwrap(ap))>TARGET)

    if idx==4% have calculated P0, P1 and P2
        %for gradient calc: X-> dimension=ST2; Y^ dimension=T
        
        dST2=AP0(2)-AP0(1);% change in output due to change in ST2
        dT=AP0(3)-AP0(1);% change in output due to change in T
        dAF=AP0(4)-AP0(1);% change in output due to change in AF
        desired_change=sign(AP0(1))*TARGET-AP0(1);% how far to go to reach target
        GMAG=sqrt(dST2^2+dT^2+dAF^2);% gradient magnitude
        if GMAG==0
            delta_ST2=delta_ST2+ST2_EPSILON;% move a little along the gradient to get the next value of st2_0
            delta_T=delta_T+T_EPSILON; % etc   
            delta_AF=delta_AF+10*AF_EPSILON; 
        else
%         GTHETA=atan(dT/dST2);% gradient theta
            delta_ST2=abs(dST2)*ST2_EPSILON;% move a little along the gradient to get the next value of st2_0
            delta_T=abs(dT)*T_EPSILON; % etc   
            delta_AF=abs(dAF)*AF_EPSILON;
        end% if GMAG==0
        st2_0=st2_0+delta_ST2;
        %st2_0=st2_0+delta_ST2/(1+(((st2_0+delta_ST2+INC))/.39).^22);
        t_0=t_0+delta_T;
        %t_0=t_0+delta_T/(1+(((t_0+delta_T+INC))/.32).^4); % E=.5, pow=22
        af_0=af_0+delta_AF;
%         Ve=t_0*(.18*af_0);% velocity t end of first acceleration pulse
%         CP=Ve*(t_0+st2_0);% cart position due to acceleration pulses + cruise stage  
        CP=0.18*t_0^2*af_0+0.18*t_0*af_0*st2_0;
         if abs(CP)>CP_D% if the proposed parameters cause the cart to move close to endpoints, readjust parameters
            saf=sign(af_0);
            af_0=abs(af_0);             
            b=(2^(1/3)*5^(2/3)*CP_D^(1/3))/(3^(2/3)*(t_0*af_0*(t_0+st2_0))^(1/3));%multiply each parameter by this fraction to get the gradient step to incur desired CP_D            
            t_0=t_0*b;
            af_0=af_0*b;
            st2_0=st2_0*b; 
            af_0=af_0*saf;
          %change t_0, the most dynamic parameter, to make CP=0.975*MAX_P.
          %The solution is the positive root (negative time duration doesn't make sense) of the equation for CP, solved
          %for t_0, where CP=0.975*MAX_P
          %t_0=(sqrt(af_0*(200*.975*MAX_P+9*af_0*st2_0^2))-(3*af_0*st2_0))/(6*af_0);
        end%  if CP>CP_D
        ST2=st2_0;
        T=t_0; %expeiment: .3 is first T that causes overshoot
        AF=af_0;
        idx=0;
        STO=[STO;AP0 ST2 T AF]% store the P0 values, monitor approach rate
        prev_AP0=AP0;
        AP0=[];
    end
%     if idx>=2% there are 2 data points in AP0
%         dap0=AP0(end)-AP0(end-1);% change in output over epsilon parameter change
%         st2_delta=[st2_delta .8*st2_epsilon*(sign(AP0(2))*TARGET-AP0(2))/dap0];
%         t_delta=[t_delta .8*t_epsilon*(sign(AP0(2))*TARGET-AP0(2))/dap0];
%         if (st2_delta(end)<st2_epsilon) || (t_delta(end)<t_epsilon)% switch to close-in regime
%             STOP=1;
%             break;
%         end
%     else
    if idx==1
        CP=0.18*t_0^2*af_0+0.18*t_0*af_0*(st2_0+ST2_INC);
        if abs(CP)>CP_D
            %then calculate increment to make the cart movement meet maxima
            old_inc=ST2_INC;
            saf=sign(af_0);
            af_0=abs(af_0);            
            ST2_INC=(50*CP_D)/(9*t_0*af_0)-t_0-st2_0;
            inc_ratio=ST2_INC/old_inc; % old_inc will always be larger at this point
            %reduce other increment variables to maintain equal increments.
            T_INC=T_INC*inc_ratio;
            AF_INC=AF_INC*inc_ratio;
            af_0=af_0*saf;
        end
        ST2=st2_0+ST2_INC;  
        T=t_0;
        AF=af_0;
    elseif idx==2
        CP=0.18*(t_0+T_INC)^2*af_0+0.18*(t_0+T_INC)*af_0*st2_0;
        if abs(CP)>CP_D
            old_inc=T_INC;
            saf=sign(af_0);
            af_0=abs(af_0);
            T_INC=(sqrt(af_0*(200*CP_D+9*af_0*st2_0^2))-3*af_0*st2_0)/(6*af_0)-t_0;
            inc_ratio=T_INC/old_inc;
            ST2_INC=ST2_INC*inc_ratio;
            AF_INC=AF_INC*inc_ratio;
            af_0=af_0*saf;
        end
        T=t_0+T_INC;
        ST2=st2_0;
        AF=af_0;
    elseif idx==3
        CP=0.18*t_0^2*af_0+0.18*t_0*(af_0+AF_INC)*st2_0;
        if abs(CP)>CP_D
            old_inc=AF_INC;
            saf=sign(af_0);
            af_0=abs(af_0);            
            AF_INC=(50*CP_D)/(9*t_0*(t_0+st2_0))-af_0;
            inc_ratio=AF_INC/old_inc;
            ST2_INC=ST2_INC*inc_ratio;
            T_INC=T_INC*inc_ratio;
            af_0=af_0*saf;
        end        
        AF=af_0+AF_INC;
        ST2=st2_0;
        T=t_0;
    %if idx==4, then we're calculating the actual next gradient step
    end% if
    
%     end% if idx>=2
    TLDST2(1,1,AAorder(1))=ST2;
    TLDT(1,1,AAorder(1))=T;
    TLDAF(1,1,AAorder(1))=AF;
    idx=idx+1; 
end% while ~STOP

%%


LL=10;% latch length: used to initialize the length of arrays that accumulate the sensor signal features
ap_vals=0;% initialize vector of angular position values at the end of each pulse 



AF=200;% applied force impulse amplitudes
St=.1;% applied force impulse start times ( seconds)
T=.27283328;% applied force impulse periods
AF2=AF;
T2=T;
St2=.4; %550:.3801 800:1.0894,1:6
AF3=0;%7.61;
T3=.1;
St3=.3;
AF4=0;
T4=0;
St4=0;
% MA=PC;% manuver array
% MAIDX=1;% manouver array index
grad_inc=.1; % defines differential size when calculating gradients
%EPSILON=.5;% initialize epsilon
%% MAIN LOOP
ap_ta_times=0;% initialize target acquisition times
RESP=[];% record response of first feature
TARGET_ACQUIRED=0;
while ~TARGET_ACQUIRED  % repaet operation until angular position target is rached at least once
    for inst=1:3
        if inst==1% define the base point in parameter space
            PC=[AF St T;-AF2 St+T+St2 T2;AF3 St+St2+T+T2+St3 T3;AF4 St+St2+St3+T+T2+T3+St4 T4];% point one
        elseif inst==2% move along first parameter and define second point inparameter space
            if (T+grad_inc)>sqrt(MAX_P/(AF/MASS)) %for now, asume the acceleration pulse starts at cart position 0.  Need to be able to issue a mirror pulse to stop cart before it travels MAX_P
                T=sqrt(MAX_P/(AF/MASS));
                PC=[AF,St,T];
            else
                PC=[AF,St,T+grad_inc];
            end
        elseif inst==3% move along secons parameter and define thir point in parameter space
            if T>sqrt(MAX_P/((AF+grad_inc)/MASS)) %for now, asume the acceleration pulse starts at cart position 0.  Need to be able to issue a mirror pulse to stop cart before it travels MAX_P
                T=sqrt(MAX_P/((AF+grad_inc)/MASS));% limit pulse width to be stoppable
                PC=[AF+grad_inc,St,T];
            else
                PC=[AF+grad_inc,St,T];
            end                        
        end 
%        St+T applied force end time
        %%%
        %%% Make the time parameter dependant on dynamics of system: want
        %%% to move from feature to feature ( peak to peak in system
        %%% response) so only simulate times that include the minimum
        %%% number of features
        % AP stands for ANGULAR POSITION of pendulum arm
        [t,x,d1,d2,d3,d4,cp,ap]=sim('ipend2',5); 
        TARGET_ACQUIRED=1;
        break
        [dift,time_index]=min(abs(t-PC(3)));% Find the simulation time nearest the stop time. This is to inde the value of angular position at end of applied force pulse
        if inst==1
            P_0=PC;
            F_0=ap(time_index); %response of system ( peak height) at P_0
            RESP=[RESP F_0];
        elseif inst==2
            P_dP=PC;
            F_dP=ap(time_index)-F_0;
        elseif inst==3
            P_dA=PC;
            F_dA=ap(time_index)-F_0;
        end 
    end% for inst
    GradComp=[F_dA,F_dP];
    GradMag=sqrt(sum(GradComp.^2));
%     delta_P=ap_peak_times(1)-T;% increase acceleration pulse width to match the current peak time
%     delta_A=delta_P*(F_dA/F_dP);
%     if ((delta_P<=.01) || (delta_A<=.01))
%         incphase=1;
%     end
%     %target of peak is +/- pi
%     %use the gradient as a linear estimate of the response to estimate how to
%     %increment P_0 to reach goal
     diff_from_goal=1.1*(pi/2)-abs(F_0);
%     %GradMag*EPSILON=diff_from_goal::Linearizing at P_0, EPSILON is the amount to go along
%     %the gradient direction to reach the goal response
    %EPSILON=diff_from_goal/GradMag;% too large a chang eis produced, need to take temporal dynamics into account ( period, etc) to conditoin these values
    delta_F=diff([0 RESP]);
%     if abs(delta_F(end))<.1
%         EPSILON=EPSILON*(.5./abs(delta_F(end)));
%     else
%         EPSILON=.5;% fixed increment.
%     end
    THETA=atan(F_dP/F_dA);% angle in radians of gradient vector from 
    delta_A=cos(THETA)*EPSILON;% how to change AF to reak goal from P_0
    delta_P=sin(THETA)*EPSILON; % etc
    AF=AF+delta_A;
    if (T+delta_P)>sqrt(MAX_P/(AF/MASS)) %for now, asume the acceleration pulse starts at cart position 0.  Need to be able to issue a mirror pulse to stop cart before it travels MAX_P
        T=sqrt(MAX_P/(AF/MASS));% limit pulse width to be stoppable
    else
        T=T+delta_P;
    end     
%     if ((delta_P>=ap_peak_times(1)) || (delta_A>MAX_A)) % As a sanity check hack, limit acceleration pulse widths to the time of the current angular position peak
%         delta_A=(ap_peak_times(1)-T)*(F_dA/F_dP);% using tangent property of triangle formed by dA, dP, gradient vectors
%         AF=AF+delta_A;
%         T=ap_peak_times(1);% increase acceleration pulse width to match the current peak time        
%     else
%         AF=AF+delta_A;
%         T=T+delta_P;
%     end
    EPSILON
    AF
    T
    RESP
    plot(RESP,'-o')
    pause(2)
    %%%TRICK: the max T that can affect F is <= feature occurance time
end% while 
%% 
while max(ap_ta_times)==0  % repaet operation until angular position target is rached at least once
    GradP0_time=[];% GradP0 is the gradient at Point 0. It's the vector of partials: [dA dS dP]
    GradP0=[];% the time vector is just to record the occurance of each maxima
    % This is the gradient with regards to the current feature, which is
    % the magnitude of the first extrema of the angular position 
    for grad_comp=1:(length(PC)+1) % lopo through the components of the gradient vector
        P1=PC;
        if grad_comp>1% first evaluate at PC
            if grad_comp==4 % don't move initial time shift ( equals 0)
                P1(grad_comp-1)=PC(grad_comp-1);
            else
                P1(grad_comp-1)=PC(grad_comp-1)+grad_inc; % increment one component of the parapeter vector by the differential increment
            end
        end
        [t,x,ap_peaks,ap_zc,ap_ta]=sim('ipend2',4);
        %ap_peaks= 
        % AP Peak Times y(1:LL)
        % AP Peak Values y(LL+1:LL*2)
        %ap_zc=
        % AP ZC Times
        % AP Velocity at ZC times
        ap_peak_times=0;
        ap_peak_vals=0;
        ap_zc_times=0;% zero crossing
        ap_zc_vel=0;% angular velocity at zero crossing

        for i=1:LL
            idx=min(find(ap_peaks(:,i)~=0));
            if isempty(idx)
               ap_peak_times(i)=0;
               ap_peak_vals(i)=0;     
            else
                ap_peak_times(i)=ap_peaks(idx,i);
                ap_peak_vals(i)=ap_peaks(idx,i+LL);        
            end% if
            idx=min(find(ap_zc(:,i)~=0));
            if isempty(idx)
               ap_zc_times(i)=0;  
               ap_zc_vel(i)=0;
            else
                ap_zc_times(i)=ap_zc(idx,i);       
                ap_zc_vel(i)=ap_zc(idx,LL+1);
            end% if   
        end% for i
        if grad_comp==1
          [row col P0_val]=find(ap_peak_vals,1,'first'); % consider the first angular position extrema for now.  Note that there may be many extrema depending on oscillation of pendulum
        else
            [row col GradP0_val]=find(ap_peak_vals,1,'first'); % consider the first angular position extrema for now.  Note that there may be many extrema depending on oscillation of pendulum
            GradP0=[GradP0 GradP0_val- P0_val];
            GradP0_time=[GradP0_time ap_peak_times(col)];            
        end        
    end% for grad_comp
end% while


%%
% TERMS:
% -GOAL
% REPETOIRE
% MANOUVER
% ACTION
% EVENT
% 
% initialize
% MA-Manouver array
% AC- Action
% RV
% 
% do until goal is achieved
%     increment counters
%     do until event measure is maximized
%         perform action
%         measure performance
%         adjust action parameters
%     add action to manouver array
% 
%     stop larning once goal is achieved
